/* DOXYGEN KOMT NOG
 * File:   sslvsion.cpp
 * Author: Clymans Kevin
 *
 * Singleton klasse voor ssl vision aansturing !
 *
 * Created on February 24, 2011, 3:14 PM
 */

#include "sslvsion.h"
using namespace std;

//observer test

sslvision* sslvision::instance = NULL;

sslvision* sslvision::getInstance() {
    if (instance == NULL) {
        instance = new sslvision();
    }
    return instance;
}

void sslvision::addObserver(myObserver& o) {
    cout << "add observer \n";
    observers.insert(&o);
}

void sslvision::removeObserver(myObserver& o) {
    observers.erase(&o);
}

void sslvision::notifyObservers() {
    set<myObserver*>::iterator itr;
    itr = observers.begin();
    int i = 0;
    for (itr; itr != observers.end(); itr++) {
      
        //balleke
        //cout << "bal update\n";
        (*itr)->Notify(0, ball.x(), ball.y());
        //blauw robot 0
        //cout << "blauw 0 update\n";
        (*itr)->Notify(1, robotb1.x(), robotb1.y());
        //blauw robot 1
        //cout << "blauw 1 update\n";
        (*itr)->Notify(2, robotb2.x(), robotb2.y());

        //geel robot 0
        //cout << "geel 0 update";
        (*itr)->Notify(3, roboty1.x(), roboty1.y());

        //geel robot 1
        //cout << "geel 1 update";
        (*itr)->Notify(4, roboty2.x(), roboty2.y());
    }
}

void sslvision::run() {
    //sslvision::sslvision(){
    cout << "init sslvision \n";
    //log init
    logger = new Log("T_sslvsion.cpp");
    logger->stuur("init");
    client.open(true);

    while (true) {
        if (client.receive(packet)) {
            //cout << "----received wrapper packet --- - \n";
            //has robot frame
            if (packet.has_detection()) {
                detection = packet.detection();
                //Get contents
                //                t_now = GetTimeSec();
                camera_id = detection.camera_id();
                frame = detection.frame_number();
                capture = detection.t_capture();
                latency_proc = (detection.t_sent() - detection.t_capture());
                latency_net = (t_now - detection.t_sent()*1000.0);
                latency_tot = t_now - detection.t_capture()*1000.0;

                balls_n = detection.balls_size();
                robots_blue_n = detection.robots_blue_size();
                robots_yellow_n = detection.robots_yellow_size();
                if (balls_n != 0) {
                    //verwachten maar 1 bal!
                    ball = detection.balls(0);
                    printf("-Ball(%2d): CONF=%4.2f POS=<%9.2f,%9.2f> \n", balls_n, ball.confidence(), ball.x(), ball.y());

                }

                //blue robot info
                if (robots_blue_n != 0) {
                    robotb1 = detection.robots_blue(0);
                    printf("Robot blauw0: CONF=%4.2f POS=<%9.2f,%9.2f> \n", robotb1.confidence(), robotb1.x(), robotb1.y());
                }
                if (robots_blue_n > 1) {
                    robotb2 = detection.robots_blue(1);
                    printf("Robot blauw1: CONF=%4.2f POS=<%9.2f,%9.2f> \n", robotb2.confidence(), robotb2.x(), robotb2.y());
                }
                //yellow robot info
                if (robots_yellow_n != 0) {
                    roboty1 = detection.robots_yellow(0);
                    printf("Robot geel0: CONF=%4.2f POS=<%9.2f,%9.2f> \n", roboty1.confidence(), roboty1.x(), roboty1.y());
                }
                if (robots_yellow_n > 1) {
                    roboty2 = detection.robots_yellow(1);
                    printf("Robot geel1: CONF=%4.2f POS=<%9.2f,%9.2f> \n", roboty2.confidence(), roboty2.x(), roboty2.y());
                }
                this->notifyObservers();
            }
            //field info
            if (packet.has_geometry()) {
                cout << "has geometry\n";
                const SSL_GeometryData &geom = packet.geometry();
                const SSL_GeometryFieldSize &field = geom.field();
                calib_n = geom.calib_size();

                for (int i = 0; i < calib_n; i++) {
                    SSL_GeometryCameraCalibration calib = geom.calib(i);
                }
            }
        }
    }

}

void sslvision::printRobotInfo(const SSL_DetectionRobot &robot) {
    sprintf(temp, "CONF=%4.2f", robot.confidence());
    logger->stuur(temp);
    if (robot.has_robot_id()) {
        cout << ("ID=%3d", robot.robot_id());
    } else {
        cout << ("ID=N/A");
    }
    cout << (" HEIGHT=%6.2f POS=<%9.2f,%9.2f> ", robot.height(), robot.x(), robot.y());
    if (robot.has_orientation()) {
        cout << ("ANGLE=%6.3f", robot.orientation());
    } else {
        cout << ("ANGLE=N/A  ");
    }
    cout << ("RAW=<%8.2f,%8.2f>\n", robot.pixel_x(), robot.pixel_y());
    //logger->stuur((),5)
}

